% 在TDL信道下使用线性补偿方法

clear all;
clc;

addpath('Func_Tx/');
addpath('Func_Rx/');
addpath('Store_Para/');
warning('off','MATLAB:structOnObject'); % used to supress warning messages in struct(obj) calling


%% Configuration
% common for Tx/Rx: (reflect 36.331 - 6.3.8, sidelink-related IEs)
% SL Basic Operation Parameters: common for communication and broadcast (if present)
NSLRB                           = 100;                     % Sidelink bandwidth configuration in RBs: 6, 15, 25, 50, 100
NSLID                           = 301;                    % SLSSID: Physical layer sidelink synchronization identity: 0..335
slMode                          = 4;                      % Sidelink Mode: 3 (V2V-scheduled) or 4 (V2V autonomous sensing)

cp_Len_r12                      = 'Normal';                % Normal or Extended CP lengh, 'Normal' / 'Extended'
% SL V2X COMMUNICATION Resources Pool Configuration 
PSSCH_RB_Size                    = 96;                   % Indicates the number of PRBs of Channel Used {n4, n5, n6, n8, n9, n10, n12, n15, n16, n18, n20, n25, n30, n48, n50, n72, n75, n96}
PSSCH_RB_Start                   = 0;                    % Indicates the start RB of PSSCH, (PSSCH_RB_Start + PSSCH_RB_Size) should be less than (NSLRB - 2)

numTotSubframes = 20;      % Generate subframe number, PSBCH is generated every 160 subframes
Ncycles = 1;                % Repeat number

% 设置两个PSSCH信号帧的索引位置，如下设置表示Alice和Bob间隔1ms发送PSSCH信号
l_PSSCH_selected_Alice = 10;      % PSSCH subframe index
l_PSSCH_selected_Bob=11;          % PSSCH subframe index


%% Initialization

switch NSLRB
 	case 6
        NFFT = 128;
        chanSRate = 1.92e6;
	case 15
        NFFT = 256;
        chanSRate = 3.84e6;
	case 25
        NFFT = 512;
        chanSRate = 7.68e6;
	case 50
        NFFT = 1024;
        chanSRate = 15.36e6;
	case 75
        NFFT = 1536;
        chanSRate = 23.04e6;
	case 100
    	NFFT = 2048;
        chanSRate = 30.72e6;
end

switch cp_Len_r12
	case 'Normal'
        cpLen0 = round(0.0781*NFFT);
        cpLenR = round(0.0703*NFFT);
        NSLsymb = 7;
        if slMode == 1 || slMode == 2
            l_PSBCH = [0 4 5 6 7 8 9 13]';
            l_PSBCH_DMRS = [3 10]';
        elseif slMode == 3 || slMode == 4
            l_PSBCH = [0 3 5 7 8 10 13]';
            l_PSBCH_DMRS = [4 6 9]';                    
        end
            
    case 'Extended'
        cpLen0 = round(0.25*NFFT);
        cpLenR = round(0.25*NFFT);
        NSLsymb = 6;
        if slMode == 1 || slMode == 2
            l_PSBCH = [3 4 5 6 7 11]';
            l_PSBCH_DMRS = [2 8]';
        elseif slMode == 3 || slMode == 4
            error('Combination of Extended CP and SL Mode 3 or 4 (V2V) configuration not suppported.');                    
        end
        
end

PSCCH_DMRS_symbloc_perSubframe   = [2 5 8 11]';               % PSCCH DMRS Symbol locations per subframe for PSCCH
PSCCH_symbloc_perSubframe        = [0 1 3 4 6 7 9 10 12 13]'; % PSCCH Symbol locations per subframe for PSCCH
PSSCH_DMRS_symbloc_perSubframe   = [2 5 8 11]';               % PSSCH DMRS Symbol locations per subframe for PSCCH
PSSCH_symbloc_perSubframe        = [0 1 3 4 6 7 9 10 12 13]'; % PSSCH Symbol locations per subframe for PSCCH

[PSSS_Time_Domain, SSSS_Time_Domain_0, SSSS_Time_Domain_1, PSBCH_Sync_Grid] =  Function_SL_Sync_Signal_Generation(slMode, NSLRB, NSLID, NFFT, cpLenR, NSLsymb, cp_Len_r12);

%% 生成信号帧

Samples_per_Subframe_Tx = 2*NSLsymb*NFFT + 2*cpLen0 + 2*(NSLsymb-1)*cpLenR;
tx_output =  0*complex(ones(Ncycles*Samples_per_Subframe_Tx*numTotSubframes,1), ones(Ncycles*Samples_per_Subframe_Tx*numTotSubframes,1));

for cycleIx=0:Ncycles-1
    for tested_subframe = 0:numTotSubframes-1
        % initialize signal for current subframe
        tx_output_sf = 0*complex(ones(Samples_per_Subframe_Tx,1),ones(Samples_per_Subframe_Tx,1));

        % subframe generation
        % Case 1: Reference Subframe
        if(tested_subframe == 0)
            [tx_output_sf, PSBCH_FFT_Template] = Function_Create_PSBCH_Subframe (NSLsymb, NFFT, cpLen0, cpLenR, NSLRB, l_PSBCH, l_PSBCH_DMRS, NSLID, PSBCH_Sync_Grid);
            % Power normalization for simulation
            [tx_output_sf] = Function_Power_Normalization(tx_output_sf);
        % Case 2: V2X Communication Subframe
        elseif(tested_subframe == l_PSSCH_selected_Alice)   
            % create full signal and update for sps
            [tx_output_sf, PSSCH_FFT_Template] = Function_Create_PSSCH_Subframe (NSLsymb, NFFT, cpLen0, cpLenR, NSLRB, NSLID, PSSCH_RB_Size, PSSCH_RB_Start);
            % Power normalization for simulation
            [tx_output_sf] = Function_Power_Normalization(tx_output_sf);
        % type of subframe
        elseif(tested_subframe == l_PSSCH_selected_Bob)   
            % create full signal and update for sps
            [tx_output_sf, PSSCH_FFT_Template] = Function_Create_PSSCH_Subframe (NSLsymb, NFFT, cpLen0, cpLenR, NSLRB, NSLID, PSSCH_RB_Size, PSSCH_RB_Start);
            % Power normalization for simulation
            [tx_output_sf] = Function_Power_Normalization(tx_output_sf);
        end % 
        % total tx waveform loading: broadcast or comm or nothing
        tx_output(cycleIx*numTotSubframes*Samples_per_Subframe_Tx+tested_subframe*Samples_per_Subframe_Tx+1:cycleIx*numTotSubframes*Samples_per_Subframe_Tx+(tested_subframe+1)*Samples_per_Subframe_Tx,1) = tx_output_sf;
    end
end

%% 过TDL信道

Samples_per_subframe_Rx = Samples_per_Subframe_Tx;
% Add time offset 
% toff = randi([0,2*Samples_per_subframe_Rx],1,1); % pick a random offset
% % toff = 1000;
% rx_input = [zeros(toff,1); tx_output]; % induce it to the waveform
% fprintf('Simulation Time Offset: %i\n', toff);


% Add TDL Channel
Moving_Speed = 30;   % Km/Hour
Carrier_Frequency = 5.9e9;    % MHz
Channel_Seed =round(rand(1,1)*100);
c = physconst('lightspeed'); % speed of light in m/s
Doppler_Fd = Moving_Speed*1000/3600*Carrier_Frequency/c;

Channel_TDL = nrTDLChannel;

Channel_TDL.DelayProfile = 'TDL-D';     %  'TDL-A', 'TDL-B', 'TDL-C' for NLOS, 'TDL-D', 'TDL-E' for LOS
Channel_TDL.DelaySpread = 93*1e-9;      %Desired RMS delay spread in seconds---UMi, Short:45ns, Normal:93ns, Long:316ns 

Channel_TDL.KFactorScaling = true;      % false for 'TDL-A', 'TDL-B', 'TDL-C', true for 'TDL-D', 'TDL-E'
Channel_TDL.KFactor = 13.3;               % 13.3 for 'TDL-D', 22 for 'TDL-E'

Channel_TDL.MaximumDopplerShift = Doppler_Fd;
Channel_TDL.SampleRate = chanSRate;

Channel_TDL.NumTransmitAntennas = 1;
Channel_TDL.NumReceiveAntennas = 1;
Channel_TDL.Seed = Channel_Seed;

rx_input = Channel_TDL(tx_output);

% 添加噪声
SNR_target_dB = 30; % set SNR, should higher than 10dB
noise = sqrt((1/2)*10^(-SNR_target_dB/10))*complex(randn(length(rx_input),1), randn(length(rx_input),1)); % generate noise
rx_input = rx_input + noise; % induce it to the waveform
fprintf('Simulation SNR: %i dB\n', SNR_target_dB);

% 添加频偏
Frequency_Offset = 0;    % Hz
foff = Frequency_Offset / chanSRate * 2 * pi;
[rx_input] = Function_Frequency_Offset_Compensationn(rx_input, foff);    % induce it to the waveform
fprintf('Simulation Physical Frequency Offset: %f Hz\n', Frequency_Offset);

%% 信道估计

Process_Block_Size = 8192;          % Number of samples for each process, should be the N*NFFT_Signal_Detection


Store_Block_Size = 30720;           % Number of stored samples for demodulation (each frame)

NFFT_Signal_Detection = 1024;       % FFT signal detection window length
Window_Thres_PSBCH = 4;           	% FFT signal detection window threshold for PSBCH detection (Need to test in real environment)
Window_Thres_PSSCH = 3;           	% FFT signal detection window threshold for PSSCH detection (Need to test in real environment)

Signal_PSBCH_Indication = -1;       % -1 for no signal detected; 1 for signal detected; 0 for possible signal detected
Signal_PSSCH_Indication = -1;    	% -1 for no signal detected; 1 for signal detected; 0 for possible signal detected
Process_PSBCH_Detection = 1;        % 1 for process PSBCH detection; 0 for no need of PSBCH detection; 2 for detected PSBCH signal, still need to get PSBCH signal
Process_PSSCH_Detection = 0;        % 1 for process PSSCH detection; 0 for no need of PSSCH detection (When PSBCH is detected, Process_PSSCH_Detection set to 1); 2 for detected PSSCH signal, still need to get PSBCH signal
Simulation_Indication = 1;          % 1 for normal simulatiom, 0 for stop simulation
Simulation_Data_Index = 0;          % Count simulation sample processed
Syn_OK_PSBCH_Indication = 0;        % Indication of PSBCH Synchronization
Syn_OK_PSSCH_Indication = 0;        % Indication of PSSCH Synchronization
Data_Start_Index = 0;               % Estimated data start index (from PSSS signal)
Desired_PSSCH_Start = 0;            % Desired PSSCH start index (from estimation of Data_Start_Index, PSSCH subframe index and subframe length )
PSSCH_Syn_Search_Length = 100;   	% PSSCH time synchronization search length (just around Desired_PSSCH_Start)
PSSCH_Self_Corr_Syn_Thres = 0.6; 	% PSSCH time synchronization self-corr threshold

PSSS_Length = NFFT + cpLenR;        % PSSS OFDM symbol length

Max_Block_Number = floor(Process_Block_Size / NFFT_Signal_Detection);   % Number of blocks for FFT signal detection

Previous_Block = zeros(Process_Block_Size,1);   % Temp stored sample of previous block
Current_Block = zeros(Process_Block_Size,1);    % Temp stored sample of current block

Rx_PSSS_Wave = zeros(2*PSSS_Length,1);      % Stored PSSS time domain waveform
Get_Signal_Temp_Store = zeros(Process_Block_Size + 2*NFFT_Signal_Detection,1);    % Temp stored signal for synchronization and detection
Get_PSBCH_Signal_Store = zeros(Store_Block_Size,1);     % Stored PSBCH frame
Get_PSSCH_Signal_Store = zeros(Store_Block_Size,1);     % Stored PSSCH frame

PSBCH_FFT_Template = resample(PSBCH_FFT_Template,NFFT_Signal_Detection,NFFT);   % PSBCH_FFT_Template is generated by NFFT points, get the template for NFFT_Signal_Detection points FFT
PSSCH_FFT_Template = resample(PSSCH_FFT_Template,NFFT_Signal_Detection,NFFT);   % PSSCH_FFT_Template is generated by NFFT points, get the template for NFFT_Signal_Detection points FFT


while(Simulation_Indication>0)
    %% Get samples
    Current_Block = rx_input(Simulation_Data_Index+1:Simulation_Data_Index+Process_Block_Size);
    Simulation_Data_Index = Simulation_Data_Index + Process_Block_Size;
    if(Simulation_Data_Index>(length(rx_input) - Process_Block_Size))
        Simulation_Indication = 0;
    end
    
    %% Process PSBCH Detection
    if(Process_PSBCH_Detection == 1)
        Signal_Detection_Window = PSBCH_FFT_Template;
        
        % Process PSBCH signal detection using FFT window
        Signal_PSBCH_Indication_Old = Signal_PSBCH_Indication;
        switch Signal_PSBCH_Indication_Old
            case -1
                [Signal_PSBCH_Indication, First_Detection_New] = Function_Dection_Windowed_FFT(Current_Block, Signal_Detection_Window, NFFT_Signal_Detection,Window_Thres_PSBCH);
                First_Detection = First_Detection_New;
                if(First_Detection == 1)
                    Signal_PSBCH_Indication_Old = Signal_PSBCH_Indication;
                end
            case 0
                [Signal_PSBCH_Indication, First_Detection_New] = Function_Dection_Windowed_FFT(Current_Block, Signal_Detection_Window, NFFT_Signal_Detection,Window_Thres_PSBCH);
                if(Signal_PSBCH_Indication == 1 && First_Detection_New == 1)
                    Signal_PSBCH_Indication_Old = Signal_PSBCH_Indication;
                end
            case 1

        end
    
    
        if(Signal_PSBCH_Indication == 1 && Signal_PSBCH_Indication_Old == 1)
            % Set Process_PSBCH_Detection to 2, no need for signal detection, but still need to get samples from each block 
            Process_PSBCH_Detection = 2;
            Get_Data_Length_Count = 0;
            Get_Signal_Temp_Store = zeros(Process_Block_Size + 2*NFFT_Signal_Detection,1);    % Temp stored signal for synchronization and detection
            % Get signal segment from two adjacent blocks
            if(First_Detection == 1)
                Get_Data_Index = 0;
                Get_Signal_Temp_Store(Get_Data_Index+1:Get_Data_Index+(First_Detection+1)*NFFT_Signal_Detection) = Previous_Block(Process_Block_Size-(First_Detection+1)*NFFT_Signal_Detection+1:Process_Block_Size);
                Get_Data_Index = Get_Data_Index + (First_Detection+1)*NFFT_Signal_Detection;
                Get_Signal_Temp_Store(Get_Data_Index+1:Get_Data_Index+(Max_Block_Number-First_Detection+1)*NFFT_Signal_Detection) = Current_Block(1:(Max_Block_Number-First_Detection+1)*NFFT_Signal_Detection);
                Current_Block_Rest_Length = Process_Block_Size - (Max_Block_Number-First_Detection+1)*NFFT_Signal_Detection;
                Data_Start_Index = Simulation_Data_Index - Process_Block_Size - (First_Detection+1)*NFFT_Signal_Detection + 1;
            else
                Get_Data_Index = 0;
                Get_Signal_Temp_Store(Get_Data_Index+1:Get_Data_Index+(Max_Block_Number-First_Detection+2)*NFFT_Signal_Detection) = Previous_Block(Process_Block_Size-(Max_Block_Number-First_Detection+2)*NFFT_Signal_Detection+1:Process_Block_Size);
                Get_Data_Index = Get_Data_Index + (Max_Block_Number-First_Detection+2)*NFFT_Signal_Detection;
                Get_Signal_Temp_Store(Get_Data_Index+1:Get_Data_Index+First_Detection*NFFT_Signal_Detection) = Current_Block(1:First_Detection*NFFT_Signal_Detection); 
                Current_Block_Rest_Length = Process_Block_Size - First_Detection*NFFT_Signal_Detection;
                Data_Start_Index = Simulation_Data_Index - Process_Block_Size - (Max_Block_Number-First_Detection+2)*NFFT_Signal_Detection + 1;
            end

%             plot(real(Get_Signal_Temp_Store));

            % Need to add 2 PSSS detection
            [Syn_OK_PSBCH_Indication, Syn_Index_PSSS] = Function_PSSS_Detection(Get_Signal_Temp_Store, PSSS_Time_Domain);

            if(Syn_OK_PSBCH_Indication == 1)
                % Syn_Index is the synchronized point in Get_Signal_Temp_Store. Syn_Index may be negative, which means that some samples in Previous_Block are needed.
                Syn_Index = Syn_Index_PSSS - NFFT - cpLen0;
                
                % Process frequency offset estimation using PSSS samples
                Rx_PSSS_Wave = Get_Signal_Temp_Store(Syn_Index_PSSS:Syn_Index_PSSS+2*PSSS_Length-1);
                Est_Freq_Offset_PSSS = Function_Frequency_Offset_Est_PSSS (Rx_PSSS_Wave, NFFT, cpLenR);
                
                % Get global synchronized start point
                Data_Start_Index = Data_Start_Index + Syn_Index - 1;
                Desired_PSSCH_Start = Data_Start_Index + Samples_per_subframe_Rx * l_PSSCH_selected_Alice;
                
                Estimated_Frequency_Offset_Value = Est_Freq_Offset_PSSS * chanSRate / 2 / pi;
                fprintf('Estimated Frequency Offset Factor PSSS: %f\n', Est_Freq_Offset_PSSS);
                fprintf('Estimated Frequency Offset PSSS: %f Hz\n', Estimated_Frequency_Offset_Value);
                fprintf('Synchronized PSBCH Start Sample Index: %i\n', Data_Start_Index);
                fprintf('Desired PSSCH Start Sample Index: %i\n', Desired_PSSCH_Start);
                
                % Get one subframe samples
                if(Syn_Index>0)
                    Get_Max_Data_Length = Process_Block_Size + 2*NFFT_Signal_Detection - Syn_Index + 1;
                    if(Get_Max_Data_Length>=(Store_Block_Size - Get_Data_Length_Count))
                        % Process block length is enough for one subframe
                        Get_PSBCH_Signal_Store(1:Store_Block_Size) = Get_Signal_Temp_Store(Syn_Index:Syn_Index+Store_Block_Size-1);
                        Get_Data_Length_Count = Get_Data_Length_Count + Store_Block_Size;
                        % Set Process_PSBCH_Detection to 0, no need for signal detection
                        Process_PSBCH_Detection = 0;
                    else
                        % Process block length is not enough for one subframe
                        Get_PSBCH_Signal_Store(1:Get_Max_Data_Length) = Get_Signal_Temp_Store(Syn_Index:Syn_Index+Get_Max_Data_Length - 1);
                        Get_Data_Length_Count = Get_Data_Length_Count + Get_Max_Data_Length;
                        if(Current_Block_Rest_Length>0)
                            % Get rest samples from Current_Block
                            if(Current_Block_Rest_Length>(Store_Block_Size - Get_Data_Length_Count))
                                Get_PSBCH_Signal_Store(Get_Data_Length_Count+1:Store_Block_Size) = Current_Block(Process_Block_Size - Current_Block_Rest_Length + 1:Process_Block_Size - Current_Block_Rest_Length + Store_Block_Size - Get_Data_Length_Count);
                                Get_Data_Length_Count = Store_Block_Size;
                                % Set Process_PSBCH_Detection to 0, no need for signal detection
                                Process_PSBCH_Detection = 0;
                            else
                                Get_PSBCH_Signal_Store(Get_Data_Length_Count+1:Get_Data_Length_Count+Current_Block_Rest_Length) = Current_Block(Process_Block_Size - Current_Block_Rest_Length + 1:Process_Block_Size);
                                Get_Data_Length_Count = Get_Data_Length_Count + Current_Block_Rest_Length;
                            end
                        end
                    end
                else
                    % Still have some samples in Previous_Block
                    Get_PSBCH_Signal_Store(1:-Syn_Index+1) = Previous_Block(Process_Block_Size-(Max_Block_Number-First_Detection+2)*NFFT_Signal_Detection+Syn_Index:Process_Block_Size-(Max_Block_Number-First_Detection+2)*NFFT_Signal_Detection);
                    Get_Data_Length_Count = Get_Data_Length_Count - Syn_Index + 1;
                    Get_Max_Data_Length = Process_Block_Size + 2*NFFT_Signal_Detection;
                    if(Get_Max_Data_Length>=(Store_Block_Size - Get_Data_Length_Count))
                        % Process block length is enough for one subframe
                        Get_PSBCH_Signal_Store(Get_Data_Length_Count+1:Get_Data_Length_Count+Store_Block_Size) = Get_Signal_Temp_Store(1:Store_Block_Size-Get_Data_Length_Count);
                        Get_Data_Length_Count = Get_Data_Length_Count + Store_Block_Size - Get_Data_Length_Count;
                        % Set Process_PSBCH_Detection to 0, no need for signal detection
                        Process_PSBCH_Detection = 0;
                    else
                        % Process block length is not enough for one subframe
                        Get_PSBCH_Signal_Store(Get_Data_Length_Count+1:Get_Data_Length_Count+Get_Max_Data_Length) = Get_Signal_Temp_Store(1:Get_Max_Data_Length);
                        Get_Data_Length_Count = Get_Data_Length_Count + Get_Max_Data_Length;
                        if(Current_Block_Rest_Length>0)
                            % Get rest samples from Current_Block
                            if(Current_Block_Rest_Length>(Store_Block_Size - Get_Data_Length_Count))
                                Get_PSBCH_Signal_Store(Get_Data_Length_Count+1:Store_Block_Size) = Current_Block(Process_Block_Size - Current_Block_Rest_Length + 1:Process_Block_Size - Current_Block_Rest_Length + Store_Block_Size - Get_Data_Length_Count);
                                Get_Data_Length_Count = Store_Block_Size;
                                % Set Process_PSBCH_Detection to 0, no need for signal detection
                                Process_PSBCH_Detection = 0;
                            else
                                Get_PSBCH_Signal_Store(Get_Data_Length_Count+1:Get_Data_Length_Count+Current_Block_Rest_Length) = Current_Block(Process_Block_Size - Current_Block_Rest_Length + 1:Process_Block_Size);
                                Get_Data_Length_Count = Get_Data_Length_Count + Current_Block_Rest_Length;
                            end
                        end
                    end
                end
            end
        end
    else
        if(Process_PSBCH_Detection == 2)
            % Get one subframe samples from next process blocks
            Get_Max_Data_Length = Process_Block_Size;
            if(Get_Max_Data_Length>(Store_Block_Size - Get_Data_Length_Count))
                Get_PSBCH_Signal_Store(Get_Data_Length_Count+1:Store_Block_Size) = Current_Block(1:Store_Block_Size - Get_Data_Length_Count);
                Get_Data_Length_Count = Store_Block_Size;
                % Set Process_PSBCH_Detection to 0, no need for signal detection
                Process_PSBCH_Detection = 0;
            else
                Get_PSBCH_Signal_Store(Get_Data_Length_Count+1:Get_Data_Length_Count+Get_Max_Data_Length) = Current_Block(1:Get_Max_Data_Length);
                Get_Data_Length_Count = Get_Data_Length_Count + Get_Max_Data_Length;
            end
        end
    end
    
     %% Process PSSCH Detection
    if(Process_PSSCH_Detection == 1)
        % Process_PSSCH_Detection will be automatical set to 1 when PSBCH are detected and demodulated 
        Signal_Detection_Window = PSSCH_FFT_Template;
        
        % Process PSSCH signal detection using FFT window, Window_Thres could be adjusted for PSSCH detection.
        Signal_PSSCH_Indication_Old = Signal_PSSCH_Indication;
        switch Signal_PSSCH_Indication_Old
            case -1
                [Signal_PSSCH_Indication, First_Detection_PSSCH_New] = Function_Dection_Windowed_FFT(Current_Block, Signal_Detection_Window, NFFT_Signal_Detection,Window_Thres_PSSCH);
                First_Detection_PSSCH = First_Detection_PSSCH_New;
                if(First_Detection_PSSCH == 1)
                    Signal_PSSCH_Indication_Old = Signal_PSSCH_Indication;
                end
            case 0
                [Signal_PSSCH_Indication, First_Detection_PSSCH_New] = Function_Dection_Windowed_FFT(Current_Block, Signal_Detection_Window, NFFT_Signal_Detection,Window_Thres_PSSCH);
                if(Signal_PSSCH_Indication == 1 && First_Detection_PSSCH_New == 1)
                    Signal_PSSCH_Indication_Old = Signal_PSSCH_Indication;
                end
            case 1

        end
    
    
        if(Signal_PSSCH_Indication == 1 && Signal_PSSCH_Indication_Old == 1)
            % Set Process_PSSCH_Detection to 2, no need for signal detection, but still need to get samples from each block 
            Process_PSSCH_Detection = 2;
            
            Get_Data_Length_Count = 0;
            Get_Signal_Temp_Store = zeros(Process_Block_Size + 2*NFFT_Signal_Detection,1);    % Temp stored signal for synchronization and detection
            % Get signal segment from two adjacent blocks
            if(First_Detection_PSSCH == 1)
                Get_Data_Index = 0;
                Get_Signal_Temp_Store(Get_Data_Index+1:Get_Data_Index+(First_Detection_PSSCH+1)*NFFT_Signal_Detection) = Previous_Block(Process_Block_Size-(First_Detection_PSSCH+1)*NFFT_Signal_Detection+1:Process_Block_Size);
                Get_Data_Index = Get_Data_Index + (First_Detection_PSSCH+1)*NFFT_Signal_Detection;
                Get_Signal_Temp_Store(Get_Data_Index+1:Get_Data_Index+(Max_Block_Number-First_Detection_PSSCH+1)*NFFT_Signal_Detection) = Current_Block(1:(Max_Block_Number-First_Detection_PSSCH+1)*NFFT_Signal_Detection);
                Current_Block_Rest_Length = Process_Block_Size - (Max_Block_Number-First_Detection_PSSCH+1)*NFFT_Signal_Detection;
                Data_Start_Index = Simulation_Data_Index - Process_Block_Size - (First_Detection_PSSCH+1)*NFFT_Signal_Detection + 1;
            else
                Get_Data_Index = 0;
                Get_Signal_Temp_Store(Get_Data_Index+1:Get_Data_Index+(Max_Block_Number-First_Detection_PSSCH+2)*NFFT_Signal_Detection) = Previous_Block(Process_Block_Size-(Max_Block_Number-First_Detection_PSSCH+2)*NFFT_Signal_Detection+1:Process_Block_Size);
                Get_Data_Index = Get_Data_Index + (Max_Block_Number-First_Detection_PSSCH+2)*NFFT_Signal_Detection;
                Get_Signal_Temp_Store(Get_Data_Index+1:Get_Data_Index+First_Detection_PSSCH*NFFT_Signal_Detection) = Current_Block(1:First_Detection_PSSCH*NFFT_Signal_Detection); 
                Current_Block_Rest_Length = Process_Block_Size - First_Detection_PSSCH*NFFT_Signal_Detection;
                Data_Start_Index = Simulation_Data_Index - Process_Block_Size - (Max_Block_Number-First_Detection_PSSCH+2)*NFFT_Signal_Detection + 1;
            end
%             plot(real(Get_Signal_Temp_Store));
            
            % Time synchronization using the information from Estimated_Syn_Index
            Estimated_Syn_Index = Desired_PSSCH_Start - Data_Start_Index;
            Syn_Start = Estimated_Syn_Index - PSSCH_Syn_Search_Length;
            if(Syn_Start<0)
                Syn_Start = 0;
            end
            Corr_Length = 2*PSSCH_Syn_Search_Length;
            Syn_Length = cpLen0;
            Syn_Interval = NFFT;
            Syn_Period = 1;
            
            [Syn_Index_Corr, Syn_OK_PSSCH_Indication] = Function_Synchronization_Self_Corr(Get_Signal_Temp_Store, Syn_Start, Syn_Length, Syn_Interval, Syn_Period, Corr_Length, PSSCH_Self_Corr_Syn_Thres);
            Estimated_Syn_Index = Syn_Start + Syn_Index_Corr + 1;
            
            if(Syn_OK_PSSCH_Indication == 1)
                Syn_Index = Estimated_Syn_Index;
                
                % Get global synchronized start point
                Data_Start_Index = Data_Start_Index + Syn_Index - 1;
                fprintf('Estimated PSSCH Start Sample Index: %i\n', Data_Start_Index);
                
                % Get one subframe samples
                if(Syn_Index>0)
                    Get_Max_Data_Length = Process_Block_Size + 2*NFFT_Signal_Detection - Syn_Index + 1;
                    if(Get_Max_Data_Length>=(Store_Block_Size - Get_Data_Length_Count))
                        % Process block length is enough for one subframe
                        Get_PSSCH_Signal_Store(1:Store_Block_Size) = Get_Signal_Temp_Store(Syn_Index:Syn_Index+Store_Block_Size-1);
                        Get_Data_Length_Count = Get_Data_Length_Count + Store_Block_Size;
                        % Set Process_PSSCH_Detection to 0, no need for signal detection
                        Process_PSSCH_Detection = 0;
                    else
                        % Process block length is not enough for one subframe
                        Get_PSSCH_Signal_Store(1:Get_Max_Data_Length) = Get_Signal_Temp_Store(Syn_Index:Syn_Index+Get_Max_Data_Length - 1);
                        Get_Data_Length_Count = Get_Data_Length_Count + Get_Max_Data_Length;
                        if(Current_Block_Rest_Length>0)
                            % Get rest samples from Current_Block
                            if(Current_Block_Rest_Length>(Store_Block_Size - Get_Data_Length_Count))
                                Get_PSSCH_Signal_Store(Get_Data_Length_Count+1:Store_Block_Size) = Current_Block(Process_Block_Size - Current_Block_Rest_Length + 1:Process_Block_Size - Current_Block_Rest_Length + Store_Block_Size - Get_Data_Length_Count);
                                Get_Data_Length_Count = Store_Block_Size;
                                % Set Process_PSSCH_Detection to 0, no need for signal detection
                                Process_PSSCH_Detection = 0;
                            else
                                Get_PSSCH_Signal_Store(Get_Data_Length_Count+1:Get_Data_Length_Count+Current_Block_Rest_Length) = Current_Block(Process_Block_Size - Current_Block_Rest_Length + 1:Process_Block_Size);
                                Get_Data_Length_Count = Get_Data_Length_Count + Current_Block_Rest_Length;
                            end
                        end
                    end
                else
                    % Still have some samples in Previous_Block
                    Get_PSSCH_Signal_Store(1:-Syn_Index+1) = Previous_Block(Process_Block_Size-(Max_Block_Number-First_Detection_PSSCH+2)*NFFT_Signal_Detection+Syn_Index:Process_Block_Size-(Max_Block_Number-First_Detection_PSSCH+2)*NFFT_Signal_Detection);
                    Get_Data_Length_Count = Get_Data_Length_Count - Syn_Index + 1;
                    Get_Max_Data_Length = Process_Block_Size + 2*NFFT_Signal_Detection;
                    if(Get_Max_Data_Length>=(Store_Block_Size - Get_Data_Length_Count))
                        % Process block length is enough for one subframe
                        Get_PSSCH_Signal_Store(Get_Data_Length_Count+1:Get_Data_Length_Count+Store_Block_Size) = Get_Signal_Temp_Store(1:Store_Block_Size-Get_Data_Length_Count);
                        Get_Data_Length_Count = Get_Data_Length_Count + Store_Block_Size - Get_Data_Length_Count;
                        % Set Process_PSSCH_Detection to 0, no need for signal detection
                        Process_PSSCH_Detection = 0;
                    else
                        % Process block length is not enough for one subframe
                        Get_PSSCH_Signal_Store(Get_Data_Length_Count+1:Get_Data_Length_Count+Get_Max_Data_Length) = Get_Signal_Temp_Store(1:Get_Max_Data_Length);
                        Get_Data_Length_Count = Get_Data_Length_Count + Get_Max_Data_Length;
                        if(Current_Block_Rest_Length>0)
                            % Get rest samples from Current_Block
                            if(Current_Block_Rest_Length>(Store_Block_Size - Get_Data_Length_Count))
                                Get_PSSCH_Signal_Store(Get_Data_Length_Count+1:Store_Block_Size) = Current_Block(Process_Block_Size - Current_Block_Rest_Length + 1:Process_Block_Size - Current_Block_Rest_Length + Store_Block_Size - Get_Data_Length_Count);
                                Get_Data_Length_Count = Store_Block_Size;
                                % Set Process_PSSCH_Detection to 0, no need for signal detection
                                Process_PSSCH_Detection = 0;
                            else
                                Get_PSSCH_Signal_Store(Get_Data_Length_Count+1:Get_Data_Length_Count+Current_Block_Rest_Length) = Current_Block(Process_Block_Size - Current_Block_Rest_Length + 1:Process_Block_Size);
                                Get_Data_Length_Count = Get_Data_Length_Count + Current_Block_Rest_Length;
                            end
                        end
                    end
                end
            else
                Process_PSSCH_Detection = 1;
            end
        end
    else
        if(Process_PSSCH_Detection == 2)
            % Get one subframe samples from next process blocks
            Get_Max_Data_Length = Process_Block_Size;
            if(Get_Max_Data_Length>(Store_Block_Size - Get_Data_Length_Count))
                Get_PSSCH_Signal_Store(Get_Data_Length_Count+1:Store_Block_Size) = Current_Block(1:Store_Block_Size - Get_Data_Length_Count);
                index=Process_Block_Size-(Store_Block_Size - Get_Data_Length_Count);
                Get_Data_Length_Count = Store_Block_Size;
                % Set Process_PSSCH_Detection to 0, no need for signal detection
                Process_PSSCH_Detection = 0;
                
            else
                Get_PSSCH_Signal_Store(Get_Data_Length_Count+1:Get_Data_Length_Count+Get_Max_Data_Length) = Current_Block(1:Get_Max_Data_Length);
                Get_Data_Length_Count = Get_Data_Length_Count + Get_Max_Data_Length;
            end
        end
    end
    
    % Transfer sample in current block to previous block in each block process
    Previous_Block = Current_Block;
    
    if(Process_PSBCH_Detection == 0 && Syn_OK_PSBCH_Indication == 1)
        % Process_PSBCH_Detection = 0 and Syn_OK_PSBCH_Indication = 1 means get whole subframe samples and signal sychronization is OK
        Get_PSBCH_Signal_Store = Function_Frequency_Offset_Compensationn(Get_PSBCH_Signal_Store, -Est_Freq_Offset_PSSS);
        [DMRS_PSBCH_Freq_Domain, DMRS_PSBCH_Channel_H] = Function_Get_PSBCH_DMRS(NSLsymb, NFFT, cpLen0, cpLenR, NSLRB, l_PSBCH_DMRS, NSLID, Get_PSBCH_Signal_Store);
        Process_PSSCH_Detection = 1;
        Syn_OK_PSBCH_Indication = 0;
    end
    
    % Bob进行频域信道估计
    if(Process_PSSCH_Detection == 0 && Syn_OK_PSSCH_Indication == 1)
        % Process_PSSCH_Detection = 0 and Syn_OK_PSSCH_Indication = 1 means get whole subframe samples and signal sychronization is OK
        Get_PSSCH_Signal_Store = Function_Frequency_Offset_Compensationn(Get_PSSCH_Signal_Store, -Est_Freq_Offset_PSSS);
        Get_CFO_Estimation_Wave = Get_PSSCH_Signal_Store(1:NFFT+cpLen0);
        Get_CFO_Estimation_Wave(1:NFFT+cpLen0) = Get_CFO_Estimation_Wave(1:NFFT+cpLen0).*exp(-2i*pi*(-cpLen0:NFFT-1)'/NFFT/2);
    	[Est_Freq_Offset_OFDM_CP] = Function_Frequency_Offset_Estimation(Get_CFO_Estimation_Wave, 0, cpLen0, NFFT);
        Get_PSSCH_Signal_Store = Function_Frequency_Offset_Compensationn(Get_PSSCH_Signal_Store, -Est_Freq_Offset_OFDM_CP);
        [DMRS_PSCCH_Freq_Domain, DMRS_PSCCH_Channel_H, DMRS_PSSCH_Freq_Domain, DMRS_PSSCH_H_Bob] = Function_Get_PSSCH_DMRS(NSLsymb, NFFT, cpLen0, cpLenR, NSLRB, PSSCH_DMRS_symbloc_perSubframe, NSLID, PSSCH_RB_Size, PSSCH_RB_Start, Get_PSSCH_Signal_Store);
        Syn_OK_PSSCH_Indication = 0;
        
        % Alice进行频域信道估计
        Get_PSSCH_Signal_Store= rx_input(Data_Start_Index+Samples_per_subframe_Rx*(l_PSSCH_selected_Bob - l_PSSCH_selected_Alice):Data_Start_Index+Samples_per_subframe_Rx*(l_PSSCH_selected_Bob - l_PSSCH_selected_Alice)+ Samples_per_subframe_Rx - 1);

        Get_PSSCH_Signal_Store = Function_Frequency_Offset_Compensationn(Get_PSSCH_Signal_Store, -Est_Freq_Offset_PSSS);
        Get_CFO_Estimation_Wave = Get_PSSCH_Signal_Store(1:NFFT+cpLen0);
        Get_CFO_Estimation_Wave(1:NFFT+cpLen0) = Get_CFO_Estimation_Wave(1:NFFT+cpLen0).*exp(-2i*pi*(-cpLen0:NFFT-1)'/NFFT/2);
    	[Est_Freq_Offset_OFDM_CP] = Function_Frequency_Offset_Estimation(Get_CFO_Estimation_Wave, 0, cpLen0, NFFT);
        Get_PSSCH_Signal_Store = Function_Frequency_Offset_Compensationn(Get_PSSCH_Signal_Store, -Est_Freq_Offset_OFDM_CP);
        [DMRS_PSCCH_Freq_Domain, DMRS_PSCCH_Channel_H, DMRS_PSSCH_Freq_Domain, DMRS_PSSCH_H_Alice] = Function_Get_PSSCH_DMRS(NSLsymb, NFFT, cpLen0, cpLenR, NSLRB, PSSCH_DMRS_symbloc_perSubframe, NSLID, PSSCH_RB_Size, PSSCH_RB_Start, Get_PSSCH_Signal_Store);
        Syn_OK_PSSCH_Indication = 0;
       
       break;
    end
end

%% 互易性补偿

% 进行滑动平均滤波消除噪声干扰
windowSize =11;
H_Bob=filter(ones(1,windowSize)/windowSize,1,DMRS_PSSCH_H_Bob);
H_Alice=filter(ones(1,windowSize)/windowSize,1,DMRS_PSSCH_H_Alice);

% 功率归一化
for i=1:4
    H_Bob(:,i)= Function_Power_Normalization(H_Bob(:,i));
    H_Alice(:,i)= Function_Power_Normalization(H_Alice(:,i));
end

% 将频域CSI转时域
IFFT_H_Bob=ifft(H_Bob);
IFFT_H_Alice=ifft(H_Alice);

% 调用线性补偿函数，获取补偿后的Bob时域CSI
% 第一段DMRS和最后一段DMRS的时间差
T_dmrs=(9/14)*0.001;
% Alice和Bob发送信号的时间间隔，单位为秒
T_Alice_Bob=(l_PSSCH_selected_Bob-l_PSSCH_selected_Alice)*0.001;
IFFT_H_Bob_Compensation=Function_Linear_Compensation(IFFT_H_Bob,T_dmrs,T_Alice_Bob);

% 将补偿后的Bob时域CSI转频域
H_Bob_Compensation=fft(IFFT_H_Bob_Compensation);

% 对补偿后的频域CSI进行功率归一化
for i=1:4
    H_Bob_Compensation(:,i)= Function_Power_Normalization(H_Bob_Compensation(:,i));
end

%% 观察补偿前后的互易性
% 计算补偿前后的CSI相关系数
fprintf('补偿前CSI的相关系数');
b=corrcoef(abs(H_Bob(:,1)),abs(H_Alice(:,1)))

fprintf('补偿后CSI的相关系数');
a=corrcoef(abs(H_Bob_Compensation(:,1)),abs(H_Alice(:,1)))

figure
plot(abs(H_Bob(:,1)),'-k');
hold on
plot(abs(H_Bob_Compensation(:,1)),'-b');
hold on
plot(abs(H_Alice(:,1)),'-r');
legend("补偿前的Bob的频域CSI幅度","Alice的频域CSI幅度","补偿后的Bob的频域CSI幅度");
hold off

